/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014, Robert Bosch LLC.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Robert Bosch nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/

#include <ros/ros.h>
#include <usb_cam/usb_cam.h>
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include <sstream>
#include <std_srvs/Empty.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <fstream>


namespace usb_cam {

class UsbCamNode
{
public:
    // private ROS node handle
    ros::NodeHandle node_;

    // shared image message
    sensor_msgs::Image img_;
    //image_transport::CameraPublisher image_pub_;

    //split image
    cv::Mat leftImg;
    cv::Mat rightImg;
    cv_bridge::CvImageConstPtr cv_ptr_;
    cv_bridge::CvImagePtr leftImgPtr_=NULL;
    cv_bridge::CvImagePtr rightImgPtr_=NULL;
    sensor_msgs::CameraInfoPtr ci_left_;
    sensor_msgs::CameraInfoPtr ci_right_;
    image_transport::Publisher image_pub_left_;
    image_transport::Publisher image_pub_right_;
    sensor_msgs::ImagePtr leftPtr ;
    sensor_msgs::ImagePtr rightPtr ;

    // parameters
    std::string video_device_name_, io_method_name_, pixel_format_name_, camera_name_, camera_info_url_, color_format_name_ ;
    //std::string start_service_name_, start_service_name_;
    bool streaming_status_;
    int image_width_, image_height_, framerate_, exposure_, brightness_, contrast_, saturation_, sharpness_, focus_,
        white_balance_, gain_;
    bool autofocus_, autoexposure_, auto_white_balance_;
    boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;

    int cam_num_;
    int num_of_eye_;
    int is_using;
    std::string config_file_;
    std::string image_pub_name_, left_image_pub_name_, right_image_pub_name_;
    int split_image_enbale_, swap_eyes_enbale_;

    UsbCam cam_;

    std::string service_start_name_, service_stop_name_;
    ros::ServiceServer service_start_, service_stop_;

    int save_image_enbale_;
    std::string save_path_;
    std::string filename;
    long long time1;
    bool save_result;
    cv::Mat save_image;


    bool service_start_cap(std_srvs::Empty::Request  &req, std_srvs::Empty::Response &res )
    {
        cam_.start_capturing();
        return true;
    }


    bool service_stop_cap( std_srvs::Empty::Request  &req, std_srvs::Empty::Response &res )
    {
        cam_.stop_capturing();
        return true;
    }

    UsbCamNode(int pub_id, std::string config_file_name) :
        node_("~")
    {
        image_transport::ImageTransport it(node_);
        //image_pub_ = it.advertiseCamera("image_raw", 1);
        node_.param("cam_num", cam_num_, -1);
        if(pub_id <= cam_num_) {
            is_using = 1;

            // grab the parameters
            node_.param(config_file_name, config_file_, std::string(""));

            FILE *fh = fopen(config_file_.c_str(),"r");
            if(fh == NULL) {
                ROS_WARN("config_file dosen't exist; wrong config_file path");
                std::cout<<config_file_<<std::endl;
                ROS_BREAK();
                return;
            }
            fclose(fh);
            cv::FileStorage fsSettings(config_file_, cv::FileStorage::READ);
            if(!fsSettings.isOpened()) {
                std::cerr << "ERROR: Wrong path to settings" << std::endl;
            }

            num_of_eye_ = fsSettings["num_of_eye"];

            //common parameters
            fsSettings["video_device"] >> video_device_name_;
            image_width_ = fsSettings["image_width"];
            image_height_ = fsSettings["image_height"];
            fsSettings["pixel_format"] >> pixel_format_name_;
            fsSettings["color_format"] >> color_format_name_;
            fsSettings["io_method"] >> io_method_name_;

            //the camera info
            fsSettings["camera_name"] >> camera_name_;
            fsSettings["camera_frame_id"] >> img_.header.frame_id;

            //image topic
            split_image_enbale_ = fsSettings["split_image_enbale"];
            swap_eyes_enbale_ = fsSettings["swap_eyes_enbale"];
            fsSettings["image_topic"] >> image_pub_name_;
            fsSettings["left_image_topic"] >> left_image_pub_name_;
            fsSettings["right_image_topic"] >> right_image_pub_name_;

            if((split_image_enbale_ == 1) && (num_of_eye_ == 2)) {
                image_pub_left_ = it.advertise(left_image_pub_name_, 100);
                image_pub_right_ = it.advertise(right_image_pub_name_, 100);
            }
            else {
                image_pub_left_ = it.advertise(image_pub_name_, 100);
                if((split_image_enbale_ == 1) && (num_of_eye_ == 1)) {
                    ROS_WARN("can't split mono, check the 'num_of_eye_'");
                    split_image_enbale_ == 0;
                }
            }

            //create Services
            fsSettings["service_start_name"] >> service_start_name_;
            fsSettings["service_stop_name"] >> service_stop_name_;
            service_start_ = node_.advertiseService(service_start_name_, &UsbCamNode::service_start_cap, this);
            service_stop_ = node_.advertiseService(service_stop_name_, &UsbCamNode::service_stop_cap, this);

            //save image
            save_image_enbale_ = fsSettings["save_image_enbale"];
            fsSettings["save_path"] >> save_path_;
            if(save_image_enbale_ == 1) {
                if(access(save_path_.c_str(), 0) != 0) {
                    ROS_ERROR("please check the save dir have been mkdir:'%s' ", save_path_.c_str());
                    save_image_enbale_ = 0;
                }
                else {
                    ROS_INFO("'%s':image saving  make sure you have cleared before", save_path_.c_str());
                }
                save_image = cv::Mat(image_height_, image_width_, CV_8UC1);
            }

            //just default param
            node_.param("brightness", brightness_, -1); //0-255, -1 "leave alone"
            node_.param("contrast", contrast_, -1); //0-255, -1 "leave alone"
            node_.param("saturation", saturation_, -1); //0-255, -1 "leave alone"
            node_.param("sharpness", sharpness_, -1); //0-255, -1 "leave alone"
            node_.param("framerate", framerate_, 30);
            // enable/disable autofocus
            node_.param("autofocus", autofocus_, false);
            node_.param("focus", focus_, -1); //0-255, -1 "leave alone"
            // enable/disable autoexposure
            node_.param("autoexposure", autoexposure_, true);
            node_.param("exposure", exposure_, 100);
            node_.param("gain", gain_, -1); //0-100?, -1 "leave alone"
            // enable/disable auto white balance temperature
            node_.param("auto_white_balance", auto_white_balance_, true);
            node_.param("white_balance", white_balance_, 4000);

            // load the camera info
            node_.param("camera_info_url", camera_info_url_, std::string(""));
            // cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));

            // check for default camera info
            // if (!cinfo_->isCalibrated())
            // {
            //   cinfo_->setCameraName(video_device_name_);
            //   sensor_msgs::CameraInfo camera_info;
            //   camera_info.header.frame_id = img_.header.frame_id;
            //   camera_info.width = image_width_;
            //   camera_info.height = image_height_;
            //   cinfo_->setCameraInfo(camera_info);
            // }

            ROS_INFO("Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS", camera_name_.c_str(), video_device_name_.c_str(),
                     image_width_, image_height_, io_method_name_.c_str(), pixel_format_name_.c_str(), framerate_);

            // set the IO method
            UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_);
            if(io_method == UsbCam::IO_METHOD_UNKNOWN)
            {
                ROS_FATAL("Unknown IO method '%s'", io_method_name_.c_str());
                node_.shutdown();
                return;
            }

            // set the pixel format
            UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(pixel_format_name_);
            if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN)
            {
                ROS_FATAL("Unknown pixel format '%s'", pixel_format_name_.c_str());
                node_.shutdown();
                return;
            }

            // set the color format
            UsbCam::color_format color_format = UsbCam::color_format_from_string(color_format_name_);
            if (color_format == UsbCam::COLOR_FORMAT_UNKNOWN)
            {
                ROS_FATAL("Unknown color format '%s'", color_format_name_.c_str());
                node_.shutdown();
                return;
            }

            // start the camera
            cam_.start(video_device_name_.c_str(), io_method, pixel_format, color_format, image_width_,
                       image_height_, framerate_);

            // set camera parameters
            if (brightness_ >= 0)
            {
                cam_.set_v4l_parameter("brightness", brightness_);
            }

            if (contrast_ >= 0)
            {
                cam_.set_v4l_parameter("contrast", contrast_);
            }

            if (saturation_ >= 0)
            {
                cam_.set_v4l_parameter("saturation", saturation_);
            }

            if (sharpness_ >= 0)
            {
                cam_.set_v4l_parameter("sharpness", sharpness_);
            }

            if (gain_ >= 0)
            {
                cam_.set_v4l_parameter("gain", gain_);
            }

            // check auto white balance
            if (auto_white_balance_)
            {
                cam_.set_v4l_parameter("white_balance_temperature_auto", 1);
            }
            else
            {
                cam_.set_v4l_parameter("white_balance_temperature_auto", 0);
                cam_.set_v4l_parameter("white_balance_temperature", white_balance_);
            }

            // check auto exposure
            if (!autoexposure_)
            {
                // turn down exposure control (from max of 3)
                cam_.set_v4l_parameter("exposure_auto", 1);
                // change the exposure level
                cam_.set_v4l_parameter("exposure_absolute", exposure_);
            }

            // check auto focus
            if (autofocus_)
            {
                cam_.set_auto_focus(1);
                cam_.set_v4l_parameter("focus_auto", 1);
            }
            else
            {
                cam_.set_v4l_parameter("focus_auto", 0);
                if (focus_ >= 0)
                {
                    cam_.set_v4l_parameter("focus_absolute", focus_);
                }
            }
        }
        else {
            is_using = 0;
        }
    }

    virtual ~UsbCamNode()
    {
        cam_.shutdown();
    }

    bool take_and_send_image()
    {
        // grab the image
        cam_.grab_image(&img_);

        // grab the camera info
        // sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
        // ci->header.frame_id = img_.header.frame_id;
        // ci->header.stamp = img_.header.stamp;

        cv_ptr_= cv_bridge::toCvCopy(img_, sensor_msgs::image_encodings::MONO8);
        if((split_image_enbale_ == 1) && (num_of_eye_ == 2)) {
            leftImg=cv_ptr_->image(cv::Rect(0,0,cv_ptr_->image.cols/2, cv_ptr_->image.rows));
            rightImg=cv_ptr_->image(cv::Rect(cv_ptr_->image.cols/2,0, cv_ptr_->image.cols/2, cv_ptr_->image.rows ));

            leftImgPtr_=cv_bridge::CvImagePtr(new cv_bridge::CvImage(cv_ptr_->header, cv_ptr_->encoding,leftImg) );
            rightImgPtr_=cv_bridge::CvImagePtr(new cv_bridge::CvImage(cv_ptr_->header, cv_ptr_->encoding,rightImg) );

            leftPtr =leftImgPtr_->toImageMsg();
            rightPtr =rightImgPtr_->toImageMsg();
            leftPtr->header=img_.header;
            rightPtr->header=img_.header;

            //change left and right
            if(swap_eyes_enbale_ == 1) {
                image_pub_right_.publish(leftPtr);
                image_pub_left_.publish(rightPtr);
            }
            else {
                image_pub_right_.publish(rightPtr);
                image_pub_left_.publish(leftPtr);
            }

        }
        else {
            image_pub_left_.publish(img_);
        }

        if(save_image_enbale_ == 1) {
            time1 = img_.header.stamp.toSec() * 1000000;
            filename = save_path_ + "/" + std::to_string(time1) + ".jpg";
            save_image = cv_ptr_->image.clone();
            save_result = cv::imwrite(filename, save_image);
            if(!save_result) {
                ROS_ERROR("Failed to save image!");
            }
        }


        // publish the image
        //image_pub_.publish(img_, *ci);

        return true;
    }

    // bool spin()
    // {
    //   ros::Rate loop_rate(this->framerate_);
    //   while (node_.ok())
    //   {
    //     if (cam_.is_capturing()) {
    //       if (!take_and_send_image()) ROS_WARN("USB camera did not respond in time.");
    //     }
    //     ros::spinOnce();
    //     loop_rate.sleep();

    //   }
    //   return true;
    // }


};

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "usb_cam");
    usb_cam::UsbCamNode a(1,"config_file_1");
    usb_cam::UsbCamNode b(2,"config_file_2");
    usb_cam::UsbCamNode c(3,"config_file_3");
    usb_cam::UsbCamNode d(4,"config_file_4");

    if(b.is_using == 0) {
        b.node_.shutdown();
    }
    if(c.is_using == 0) {
        c.node_.shutdown();
    }
    if(d.is_using == 0) {
        d.node_.shutdown();
    }

    ros::Rate loop_rate(a.framerate_);
    while (a.node_.ok())
    {
        if (a.cam_.is_capturing()) {
            if (!a.take_and_send_image()) ROS_WARN("USB camera did not respond in time.");
        }
        if ((b.is_using == 1) && (b.cam_.is_capturing())) {
            if (!b.take_and_send_image()) ROS_WARN("USB camera did not respond in time.");
        }
        if ((c.is_using == 1) && (c.cam_.is_capturing())) {
            if (!c.take_and_send_image()) ROS_WARN("USB camera did not respond in time.");
        }
        if ((d.is_using == 1) && (d.cam_.is_capturing())) {
            if (!d.take_and_send_image()) ROS_WARN("USB camera did not respond in time.");
        }
        ros::spinOnce();
        loop_rate.sleep();

    }
    return EXIT_SUCCESS;
}
